#include "Path_task.h"

//PS:PWM可调范围，0~100以内任意浮点数
float MT_L = 20.0;//左轮速度初值
float MT_R = 20.0;//右轮速度初值

uint8_t RxData;//串口接收寄存器存储变量
uint8_t MID_Speed;//基准速度

float yaw_val;//记录Yaw角
float yaw_detect;//预测yaw角

static float Velocity_IL,Velocity_IR;//左右轮速度环PID输入
static float Dif_Out;//差速环PID输出

uint8_t Motor_flag;//电机使能标志位

float M_Speed_L;//左轮测速
float M_Speed_R;//右轮测速

uint8_t beep_flag = 0;//声光提示标志位
uint8_t beep_key_flag;//按键提示音
uint32_t beep_tick = 0;//声光提示持续时间

uint8_t System_Mode;

float K_ZX = 0;

/********************特殊功能进程**********************/
void Beep_Proc()
{
    if(beep_flag == 1 || beep_key_flag == 1)
    {
        DL_GPIO_setPins(GPIO_BEEP_PORT,GPIO_BEEP_PIN_1_PIN);
    }
    else 
    {
        DL_GPIO_clearPins(GPIO_BEEP_PORT,GPIO_BEEP_PIN_1_PIN);
    }

    if(beep_flag == 1)
    {
        if(uwTick - beep_tick > 300)
        {

            beep_flag = 0;
        }
    }

    if(beep_key_flag == 1)
    {
        if(uwTick - beep_tick > 100)
        {

            beep_key_flag = 0;
        }   
    }
}

/*******************************路线进程***********************************/
//要求1
void LX_Proc_1()
{
    uint8_t temp_hd_sum;
    static uint8_t LX_state = 0;

    temp_hd_sum =  Huidu_Counter();

    if(LX_state ==0)
    {
        Speed_midset = 30 - NEncoder.right_motor_total_cnt/8000 * 15.0;

        if(temp_hd_sum >=1)
        {
            beep_tick = TIL_GetTick();
            beep_flag = 1;
            LX_state = 1;
            Motor_Off();
        }
    }
}

//要求2
void LX_Proc_2()
{
    uint8_t temp_hd_sum;
    static uint8_t LX_state = 0;
    static uint32_t LuChen_Counter;
    
    temp_hd_sum =  Huidu_Counter();

    //LuChen_Counter = (NEncoder.left_motor_total_cnt + NEncoder.right_motor_total_cnt)/2;

    if(LX_state ==0)//从A触发
    {
        Speed_midset = 40;
        if(temp_hd_sum >=1)//碰到B
        {
            Speed_midset = 35;
            LX_state = 1;
            Angle_PID_Flag = 0;
            beep_flag = 1;
            beep_tick = TIL_GetTick();
            motor_total_cnt_reset();
        }
    }
    else if(LX_state == 1)//从B出发
    {
        if(NEncoder.right_motor_total_cnt >= 5000)
        {
            if(temp_hd_sum == 0)//碰到C
            {
                Speed_midset = 40;
                yaw_detect -= LX_2_IMU_ANGEL;
                Angle_PID_Flag = 1;
                LX_state = 2;
                beep_flag = 1;beep_tick = TIL_GetTick();
            }                   
        }
    }
    else if(LX_state == 2)//从C出发
    {
        if(temp_hd_sum >= 1)//碰到D
        {
            motor_total_cnt_reset();
            Speed_midset = 35;
            Angle_PID_Flag = 0;
            LX_state = 3;
            beep_flag = 1;beep_tick = TIL_GetTick();     
        }    
    }
    else if(LX_state == 3)//从D出发
    {
        if(NEncoder.right_motor_total_cnt >= 5000)
        {
            if(temp_hd_sum == 0)//碰到A
            {
                LX_state = 4;
                beep_flag = 1;beep_tick = TIL_GetTick();
                Motor_Off();
            }                 
        }
                   
    }
}

//要求3
void LX_Proc_3()
{
    uint8_t temp_hd_sum;
    static uint8_t LX_state = 0;
    static uint8_t i;


    temp_hd_sum =  Huidu_Counter();

    if(LX_state ==0)//从A触发
    {
        Speed_midset=0;
        if(Angle_PID_Flag)
        {

            if(fabsf(Yaw - yaw_detect) <= 1.5)
            {
                Speed_midset=Speed_ZX-15;
                beep_flag = 1;beep_tick = TIL_GetTick();
                LX_state = 11;
            }            
        }
    }
    else if(LX_state == 11)
    {

        if(temp_hd_sum >=1)//碰到C
        {
            Speed_midset=Speed_WD-10;
            LX_state = 1;
            Angle_PID_Flag = 0;
            beep_flag = 1;beep_tick = TIL_GetTick();
            motor_total_cnt_reset();
        }        
    }
    else if(LX_state == 1)//从C出发
    {
        if(NEncoder.right_motor_total_cnt > 5000)
        {
            if(temp_hd_sum == 0)//碰到B
            {
                Speed_midset=Speed_ZX-15;
                yaw_detect = yaw_val + LX_4_IMU_ANGEL_1 + 0.5;
                Angle_PID_Flag = 1;
                LX_state = 2;
                beep_flag = 1;beep_tick = TIL_GetTick();
            }                    
        }
    }
    else if(LX_state == 2)//从B出发
    {
        if(temp_hd_sum >= 1)//碰到D
        {
            Speed_midset=Speed_WD-10;
            Angle_PID_Flag = 0;
            LX_state = 3;
            beep_flag = 1;     
            motor_total_cnt_reset();
        }    
    }
    else if(LX_state == 3)//从D出发
    {
        if(NEncoder.right_motor_total_cnt > 5000)
        {
            if(temp_hd_sum == 0)//碰到A
            {
                beep_flag = 1;beep_tick = TIL_GetTick();
                LX_state = 4;
                Motor_Off();
            }              
        }             
    }
}

//要求4
void LX_Proc_4()
{
    uint8_t temp_hd_sum;
    static uint8_t LX_state = 0;
    static uint8_t i;

    temp_hd_sum =  Huidu_Counter();
    K_ZX = 1-(20/Speed_ZX);

    if(LX_state ==0)//从A触发
    {
        Speed_midset=0;

        if(Angle_PID_Flag)
        {

            if(fabsf(Yaw - yaw_detect) <= 1.5)
            {
                motor_total_cnt_reset();

                Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX * K_ZX);

                beep_flag = 1;beep_tick = TIL_GetTick();
                LX_state = 11;
            }            
        }
    }
    else if(LX_state == 11)
    {

        Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX *K_ZX);

        if(NEncoder.right_motor_total_cnt > 4000)
        {
            if(temp_hd_sum >=1)//碰到C
            {
                Speed_midset=Speed_WD;
                LX_state = 1;
                Angle_PID_Flag = 0;
                beep_flag = 1;beep_tick = TIL_GetTick();
                motor_total_cnt_reset();
            }    
        }
     
    }
    else if(LX_state == 1)//从C出发
    {
        if(NEncoder.right_motor_total_cnt > 5000)
        {
            if(temp_hd_sum == 0)//碰到B
            {
                motor_total_cnt_reset();
                Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX * K_ZX);
                yaw_detect = yaw_val + LX_4_IMU_ANGEL_1 + 0.65;
                Angle_PID_Flag = 1;
                LX_state = 2;
                beep_flag = 1;beep_tick = TIL_GetTick();
            }                    
        }
    }
    else if(LX_state == 2)//从B出发
    {
        Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX * K_ZX);
        if(NEncoder.right_motor_total_cnt > 4000)
        {
            if(temp_hd_sum >= 1)//碰到D
            {
                Speed_midset=Speed_WD;
                Angle_PID_Flag = 0;
                LX_state = 3;
                beep_flag = 1;beep_tick = TIL_GetTick();     
                motor_total_cnt_reset();

                if(i == 3)
                {
                    Speed_midset = Speed_WD - 5;
                }
            }              
        }
    }
    else if(LX_state == 3)//从D出发
    {
        if(NEncoder.right_motor_total_cnt > 5000)
        {
            if(temp_hd_sum == 0)//碰到A
            {
                motor_total_cnt_reset();
                Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX * K_ZX);
                LX_state = 12;
                Angle_PID_Flag = 1;
                yaw_detect = yaw_val - LX_4_IMU_ANGEL_1 + 1.0;                
                beep_flag = 1;beep_tick = TIL_GetTick();
                if(++i>=4)
                {
                    LX_state = 4;
                    Motor_Off();
                }
            }              
        }             
    }
    else if(LX_state == 12)//从A出发
    {
        Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX * K_ZX);

        if(NEncoder.right_motor_total_cnt > 4000)
        {
            if(temp_hd_sum >= 1)//碰到C
            {
                Speed_midset=Speed_WD;
                Angle_PID_Flag = 0;
                LX_state = 1;
                beep_flag = 1;beep_tick = TIL_GetTick();     
                motor_total_cnt_reset();
            }       

        }
    }
}

void LX_Proc_Select()
{
    switch(System_Mode)
    {
        case 0:
            LX_Proc_1();
            break;
        case 1:
            LX_Proc_2();
            break;
        case 2:
            LX_Proc_3();
            break;
        case 3:
            LX_Proc_4();
            break;
    }
}

void MotorDataUpload(){//5ms
    M_Speed_R = get_right_motor_speed();
    M_Speed_L = get_left_motor_speed();    
}

void Motion_proc(){//10ms
    if(Angle_PID_Flag == 0)//未开启转向环（陀螺仪）
    {
        //Dif_Out=Turn_hd_PID();
        Dif_Out = Turn_hd_PID();
        /*左右速度环差速预设*/
        //printf("speed:%f,%f,%f\r\n",Test_Ks/10,M_Speed_L,M_Speed_R);

        //printf("JiaoDu:%f,%f,%f\r\n",Roll,Yaw,Pitch);
        Velocity_IL = MID_Speed -Dif_Out;
        Velocity_IR = MID_Speed + Dif_Out;

        /*
        if(Test_pid_flag == 0)
        {
            MT_L=Velocity_PID_L(M_Speed_L,Velocity_IL);
            MT_R=Velocity_PID_R(M_Speed_R,Velocity_IR);
        }
        else
        {
            MT_L=Velocity_PID_L(M_Speed_L,Test_Ks/10);
            MT_R=Velocity_PID_R(M_Speed_R,Test_Ks/10);                        
        }*/

        /*PWM限幅*/
        if(MT_R > 100)
        {
            MT_R = 100;
        }
        else if(MT_R < -100)
        {
            MT_R = -100;
        }

        if(MT_L > 100)
        {
            MT_L = 100;
        }
        else if(MT_L < -100)
        {
            MT_L = -100;
        }

        // Set_Speed(MT_L,MT_R);//有速度环
        Set_Speed(Velocity_IL,Velocity_IR);//无速度环
        //Set_Speed(100,-30);//无速度环
        yaw_val =Yaw;
    }
    else
    {
        Dif_Out = Turn_imu_PID(Yaw, yaw_detect);
        Velocity_IL = Speed_midset -Dif_Out; 
        Velocity_IR = Speed_midset +Dif_Out;
        Set_Speed(Velocity_IL,Velocity_IR);//速度环+陀螺仪转向环
        // Set_Speed(100,-30);//无速度环
    }
}

void data_upload_proc(){
    Get_TraceData();//循迹状态检测
}

void CarTask(){//5ms

    uint8_t i = 0;

    MotorDataUpload();//参数更新
    LX_Proc_Select();//路线选择
    Beep_Proc();//蜂鸣器报警

    if(++i==2){//10ms周期运动控制
        i=0;
        Motion_proc();
    }
}
